COMMENT ⊗   VALID 00006 PAGES
C REC  PAGE   DESCRIPTION
C00001 00001
C00002 00002	BEGIN "MOVE  - GENERATES SIMPLE TRAJECTORY AND SENDS DATA TO BLUE ARM"
C00004 00003	⊃ PROCEDURE FOR SAVING A INTEGER NUMBER IN THE DATA FILE
C00006 00004	⊃ TRANSFORM DEFINITIONS
C00008 00005	⊃ SET UP THE COEFFICIENT LIST HEADER
C00011 00006	⊃ WRITE OUT THE SEGMENT HEADER IF NOT FIRST POSITION
C00013 ENDMK
C⊗;
BEGIN "MOVE  - GENERATES SIMPLE TRAJECTORY AND SENDS DATA TO BLUE ARM"

DEFINE ⊃="COMMENT",CR="'15",LF="'12",CRLF="('15&'12)",FF="'14";

STRING NEWNME;
REAL ARRAY NEWJ[1:6];
REAL TT,DIF;
BOOLEAN FIRST,LAST;
INTEGER TIME,JOINT,MASTER;
INTEGER SEGS,TNUM,TRANS;
INTEGER JTS,I,J,K,ERROR;
INTEGER ARRAY JT[1:10];
INTEGER DUM,MECH;
STRING LST,ONE,COMMA,HEAD,DLST;
INTEGER SEGPTR,BITS,PTR;
INTEGER ARRAY DATA[1:2000];
REAL ARRAY TTRANS[1:4,1:4];
REAL ARRAY DD[1:20],OLD[1:10];
REAL T,P,FACT,TS;

EXTERNAL PROCEDURE DTERMS(REAL ARRAY DD;REFERENCE REAL TH;INTEGER ARM);
EXTERNAL PROCEDURE TLKEF3(INTEGER MASTER;INTEGER ARRAY DATA);
EXTERNAL INTEGER PROCEDURE TLKEF5(REAL ARRAY TRANS,ANGLES);

REQUIRE "BEJCZY[AL,HE]" LOAD_MODULE;
REQUIRE "FAITRG.FAI[AL,HE]" LOAD_MODULE;
REQUIRE "TLKEF3.FAI[AL,HE]" LOAD_MODULE;
REQUIRE "TLKEF5.FAI[AL,HE]" LOAD_MODULE;
⊃ PROCEDURE FOR SAVING A INTEGER NUMBER IN THE DATA FILE;

SIMPLE PROCEDURE INTOUT(INTEGER NUM);
	BEGIN
	DATA[PTR]←NUM;
	PTR←PTR+1;
	END;




⊃ PROCEDURE FOR SAVING A FLOATING POINT NUMBER IN 11 FORMAT IN THE DATA ARRAY;

SIMPLE PROCEDURE FLTOUT(REAL FNUM);
	BEGIN
	LABEL ST1,ST2,OVER,FLTEND;
	INTEGER BYTE,NUM1,NUM2;
	BYTE←'013200000002;
		START_CODE
		   	MOVE   2,FNUM;
			JUMPGE 2,ST1;
			MOVN   2,2;
 			TLO    2,'400000;
		ST1:	JFCL   2,ST2;
		ST2:	ADDI   2,4;
			JFCL   2,OVER;
     		    	DPB    2,BYTE;
			SETZ   1,;
			LSHC   1,16;
			MOVEM  1,NUM1;
			SETZ   1,;
			LSHC   1,16;
			MOVEM  1,NUM2;
		END;
	DATA[PTR]←NUM1;
	PTR←PTR+1;
	DATA[PTR]←NUM2;
	PTR←PTR+1;
	GOTO FLTEND;
OVER:	OUTSTR("ERROR-ROUNDING OVERFLOW"&CRLF);
FLTEND:	END; 
⊃ TRANSFORM DEFINITIONS;

STRING ARRAY TNAME[1:20];
REAL ARRAY ANG[1:20,1:6];

PROCEDURE TRAN(STRING NME;REAL J1,J2,J3,J4,J5,J6);
	BEGIN
	TRANS←TRANS+1;
 	TNAME[TRANS]←NME;
	ANG[TRANS,1]←J1;       ANG[TRANS,2]←J2;
	ANG[TRANS,3]←J3;   	ANG[TRANS,4]←J4;
    	ANG[TRANS,5]←J5;       ANG[TRANS,6]←J6;
	END;

	
TRANS←0;

TRAN ("PARK"    , 180.0 , -90.0 , 14.00 , -90.0 ,  90.0 ,   0.0 );
TRAN ("DPARK"   , 180.0 , -79.4 , 14.36 , -90.0 , 100.0 ,   0.0 );
TRAN ("P1"      ,  31.1 ,-114.2 , 20.65 , -90.0 ,  67.4 ,   0.0 );
TRAN ("DP1"     ,  31.1 , -99.8 , 18.69 , -90.0 ,  79.3 ,   0.0 );
TRAN ("P2"      ,  90.0 , -90.0 , 18.00 , -90.0 ,  90.0 , -90.0 );
TRAN ("P3"      ,  30.0 , -70.0 , 15.00 ,  90.0 ,   0.0 ,  90.0 );


⊃ DEFINE THE WORKING JOINTS;

	JTS←6;
	JT[1]←1;
	JT[2]←2;
	JT[3]←3;
	JT[4]←4;
	JT[5]←5;
	JT[6]←6;
	BITS←'770;
	SEGPTR←8+JTS*32;
	MECH←'4;
⊃ SET UP THE COEFFICIENT LIST HEADER;

	MASTER←'67676;
	SETFORMAT(10,3);
	OUTSTR(CRLF&CRLF&"MOVE TEST PROGRAM...."&CRLF&CRLF);
	DO BEGIN "MAINLP"
	PTR←1;
	INTOUT(BITS);
	INTOUT(0);
	INTOUT(0);
	INTOUT(0);	⊃ WOBBLE;
	FIRST←TRUE;
	LAST←FALSE;
	DO BEGIN "READMVE"
		TNUM←0;

⊃ READ IN THE TRANSFORM NAME;

		DO BEGIN "READTRN"
			IF FIRST THEN 
			    OUTSTR("TYPE IN THE START TRANSFORM NAME = ")
			ELSE OUTSTR("TYPE IN THE NEXT TRANSFORM NAME = ");
			LST←INCHWL;
			IF EQU("?",LST) THEN BEGIN
			    OUTSTR("THE EXISTING TRANSFORMS ARE CALLED:"&CRLF);
			    FOR J←1 STEP 1 UNTIL TRANS DO
				OUTSTR("   "&TNAME[J]&CRLF);
			    END
			ELSE IF EQU("DEFINE",LST) THEN BEGIN
			    OUTSTR("NEW TRANFORM NAME = ");
			    NEWNME←INCHWL;
			    OUTSTR("TYPE IN THE 6 JOINT ANGLES:"&CRLF);
			    LST←INCHWL;
			    FOR J←1 STEP 1 UNTIL 6 DO 
				NEWJ[J]←REALSCAN(LST,DUM);
			    TRAN(NEWNME,NEWJ[1],NEWJ[2],NEWJ[3],NEWJ[4],
				NEWJ[5],NEWJ[6]);
			    END
			ELSE IF EQU("PRINT",LST) THEN BEGIN
			    OUTSTR("TRANSFORM NAME = ");
			    LST←INCHWL;
			    K←0;
			    FOR J←1 STEP 1 UNTIL TRANS DO 
				IF EQU(LST,TNAME[J]) THEN K←J;
			    IF K=0 THEN OUTSTR("CAN'T FIND TRANSFORM"&CRLF)
			    ELSE BEGIN
				FOR J←1 STEP 1 UNTIL 6 DO OUTSTR(CVF(ANG[K,J]));
				OUTSTR(CRLF);
				END;
			    END
			ELSE IF EQU("HERE",LST) THEN BEGIN
			    OUTSTR("TRANSFORM NAME = ");
			    NEWNME ← INCHWL;
			    ERROR ← TLKEF5(TTRANS,NEWJ);
			    IF ERROR=0 THEN TRAN(NEWNME,NEWJ[1],NEWJ[2],
				NEWJ[3],NEWJ[4],NEWJ[5],NEWJ[6])
			    ELSE OUTSTR("CAN'T DEFINE TRANSFORM"&CRLF);
			    END
			ELSE BEGIN
			    FOR J←1 STEP 1 UNTIL TRANS DO 
				IF EQU(LST,TNAME[J]) THEN TNUM←J;
			    IF TNUM=0 THEN
				OUTSTR("...INVALID TRANS NAME..."&CRLF);
			END;
		END "READTRN" UNTIL TNUM≠0;
⊃ WRITE OUT THE SEGMENT HEADER IF NOT FIRST POSITION;

		IF ¬FIRST THEN BEGIN "SETSEG"
			OUTSTR("SEGMENT TIME IN SECONDS = ");
			LST←INCHWL;
			TT←REALSCAN(LST,DUM)*1000.0;
			TIME←TT;
			INTOUT(SEGPTR);
			INTOUT(TIME);
			INTOUT(0); INTOUT(0);

⊃ WRITE OUT THE POLYNOMIAL AND DYNAMIC COEFFICIENTS;

			FOR J←1 STEP 1 UNTIL JTS DO BEGIN
				JOINT←JT[J];
				DIF←ANG[TNUM,JOINT]-OLD[JOINT];
				FLTOUT(OLD[JOINT]);
				FLTOUT(0.0);  FLTOUT(0.0);
				FLTOUT(10.0*DIF);
				FLTOUT(-15.0*DIF);
				FLTOUT(6.0*DIF);
			END;
			DTERMS(DD,ANG[TNUM,1],MECH);
			FOR J←1 STEP 1 UNTIL JTS DO BEGIN
				K←(JT[J]-1)*2+1;
				FLTOUT(DD[K]);
				FLTOUT(DD[K+1]);
			END;
			OUTSTR("IS THIS THE FINAL SEGMENT (Y,N)? ");
			LST←INCHWL;
			IF EQU(LST,"Y") THEN BEGIN
				INTOUT(0);
				LAST←TRUE;
				END;
		END "SETSEG";
		FOR J←1 STEP 1 UNTIL 6 DO OLD[J]←ANG[TNUM,J];
		FIRST←FALSE;
	END "READMVE" UNTIL LAST;
	OUTSTR("EXECUTE THIS MOTION (Y,N)? ");
	LST←INCHWL;
	IF EQU("Y",LST) THEN DO BEGIN
		TLKEF3(MASTER,DATA);
		OUTSTR("REPEAT MOTION (Y,N) ? ");
		LST←INCHWL;
		END UNTIL ¬EQU("Y",LST);
	OUTSTR("DO ANOTHER MOTION (Y,N)? " );
	LST←INCHWL;
	END "MAINLP" UNTIL ¬EQU(LST,"Y");
    	OUTSTR("I HOPE YOU HAD A GOOD TIME..."&CRLF);

END